/// SAC-IC 配准
///
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/common_headers.h>
#include <pcl/correspondence.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/registration/ia_ransac.h>


using namespace std;

typedef pcl::PointXYZRGB PointType;
typedef pcl::PointCloud<PointType> PointCloud;
typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;


struct CmdParams {
    std::string model_filename_;
    std::string scene_filename_;
    std::string out_filename_="";
    int option_ = 1;
    float model_ss_ = (0.004f);
    float normal_radius_ = 0.02f;
    float feature_radius_ = 0.02f;
    int sac_nr_samples = 10;
    int sac_k = 30;

    bool show_keypoints_ = false;
};

void
showHelp (char *filename)
{
  std::cout << std::endl;
  std::cout << "Usage: " << filename << " model.pcd scene.pcd [Options]" << std::endl << std::endl;
  std::cout << "Options:" << std::endl;
  std::cout << "     -h:                     Show this help." << std::endl;
  std::cout << "     -k:                     Show used keypoints." << std::endl;
  std::cout << "     -c:                     Show used correspondences." << std::endl;


  std::cout << "     --option val:           (default 1)" << std::endl;
  std::cout << "     --model_ss val:         Model uniform sampling radius (default 0.04)" << std::endl;
  std::cout << "     --normal_ss val:        NormalEstimation radius (default 0.02)" << std::endl;
  std::cout << "     --feature_ss val:       FPFHEstimation radius (default 0.02)" << std::endl;
  std::cout << "     --sac_nrs val:          SampleConsensusInitialAlignment the number of samples to use during each iteration  (default 10)" << std::endl;
  std::cout << "     --sac_k val:            SampleConsensusInitialAlignment the number of neighbors to use when selecting a random feature correspondence  (default 30)" << std::endl;

  std::cout << "     -o filename:            outputfile.pcd" << std::endl;

}

void
parseCommandLine (int argc, char *argv[], CmdParams &params)
{
    //Show help
    if (pcl::console::find_switch (argc, argv, "-h"))
    {
        showHelp (argv[0]);
        exit (0);
    }

    //Model & scene filenames
    std::vector<int> filenames;
    filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
    if (filenames.size () != 2)
    {
        std::cout << "Filenames missing.\n";
        showHelp (argv[0]);
        exit (-1);
    }

    params.model_filename_ = argv[filenames[0]];
    params.scene_filename_ = argv[filenames[1]];

    //Program behavior
    if (pcl::console::find_switch (argc, argv, "-k"))
    {
        params.show_keypoints_ = true;
    }

    //General parameters

    pcl::console::parse_argument (argc, argv, "--option", params.option_);
    pcl::console::parse_argument (argc, argv, "--model_ss", params.model_ss_);
    pcl::console::parse_argument (argc, argv, "--normal_ss", params.normal_radius_);
    pcl::console::parse_argument (argc, argv, "--feature_ss", params.feature_radius_);
    pcl::console::parse_argument (argc, argv, "--sac_nrs", params.sac_nr_samples);
    pcl::console::parse_argument (argc, argv, "--sac_k", params.sac_k);

//  pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
//  pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
//  pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
//  pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
//  pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
    pcl::console::parse_argument (argc, argv, "-o", params.out_filename_);

    cout << "model_ss - Model uniform sampling radius: " << params.model_ss_ << endl;
    cout << "normal_ss -NormalEstimation radius: " << params.normal_radius_ << endl;
    cout << "feature_ss - FPFHEstimation radius: " << params.feature_radius_ << endl;
    cout << "sac_nrs: " << params.sac_nr_samples << endl;
    cout << "sac_k: " << params.sac_k << endl;
    cout << "out_filename_: " << params.out_filename_ << endl;
}

/// 第一种提取特征点的方法
void extract_keypoint1(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& keypoint,
                      float LeafSize = 0.4, float radius = 0.8, float threshold = 5) // 参数分别为：体素格网的大小，法向量计算半径，夹角阈值（度）0.4,0.8,5
{
    //体素下采样
    pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd_down(new pcl::PointCloud<pcl::PointXYZRGB>);
    voxel_grid.setInputCloud(cloud);
    voxel_grid.setLeafSize(LeafSize, LeafSize, LeafSize);
    voxel_grid.filter(*pcd_down);

    //计算每一个点的法向量
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
    n.setInputCloud(pcd_down);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    n.setSearchMethod(tree);
    //设置KD树搜索半径
    //n.setRadiusSearch (0.1);
    n.setKSearch(10);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    n.compute(*normals);

    float Angle = 0.0;
    float Average_Sum_AngleK = 0.0;//定义邻域内K个点法向量夹角的平均值
    vector<int>indexes;
    //--------------计算法向量夹角及夹角均值----------------
    pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;  //建立kdtree对象
    kdtree.setInputCloud(pcd_down); //设置需要建立kdtree的点云指针

    vector<int> pointIdxRadiusSearch;  //保存每个近邻点的索引
    vector<float> pointRadiusSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
    pcl::PointXYZRGB searchPoint;
    for (size_t i = 0; i < pcd_down->points.size(); ++i) {
        searchPoint = pcd_down->points[i];
        if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
        {
            float Sum_AngleK = 0.0;//定义K个邻近的点法向夹角之和
            /*计算法向量的夹角*/
            for (size_t m = 0; m < pointIdxRadiusSearch.size(); ++m) {

                Eigen::Vector3f
                        v1(normals->points[i].data_n[0],
                        normals->points[i].data_n[1],
                        normals->points[i].data_n[2]),

                        v2(normals->points[pointIdxRadiusSearch[m]].data_n[0],
                        normals->points[pointIdxRadiusSearch[m]].data_n[1],
                        normals->points[pointIdxRadiusSearch[m]].data_n[2]);

                Angle = pcl::getAngle3D(v1, v2, true);

            }
            Sum_AngleK += Angle;//邻域夹角之和
            Average_Sum_AngleK = Sum_AngleK / pointIdxRadiusSearch.size();//邻域夹角均值

            //-----------------提取特征点--------------------
            float t = pcl::deg2rad(threshold);
            if (Average_Sum_AngleK > t) {
                indexes.push_back(i);
            }
        }
    }
    pcl::copyPointCloud(*pcd_down, indexes, *keypoint);

    cout << "提取的特征点个数:" << keypoint->points.size() << endl;
};

void extractKeyPoints2(pcl::PointCloud<PointType>::Ptr &model, pcl::PointCloud<PointType>::Ptr &model_keypoints, float radius)
{
    pcl::UniformSampling<PointType> uniform_sampling;
    uniform_sampling.setInputCloud (model);
    uniform_sampling.setRadiusSearch (radius);
    uniform_sampling.filter (*model_keypoints);
    std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
}


void compute_fpfh_feature1(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& keypoint, LocalFeatures::Ptr &fpfh)
{
    // 法向量估计
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
    n.setInputCloud(keypoint);
    n.setSearchMethod(tree);
    n.setKSearch(10);
    //n.setRadiusSearch(0.0001);
    n.compute(*normals);

    //------------------FPFH估计-------------------------
    //LocalFeatures::Ptr fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
    pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8); //指定8核计算
    f.setInputCloud(keypoint);
    f.setInputNormals(normals);
    f.setSearchMethod(tree);
    f.setKSearch(10);
    //f.setRadiusSearch(0.0012);
    f.compute(*fpfh);
}

void computeFpfhFeature2(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& keypoint, LocalFeatures::Ptr &fpfh, float normal_r, float feature_r)
{
    // 法向量估计
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n;
    n.setInputCloud(keypoint);
    n.setSearchMethod(tree);
    // n.setKSearch(10);
    n.setRadiusSearch(normal_r);
    n.compute(*normals);

    //------------------FPFH估计-------------------------
    pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8); //指定8核计算
    f.setInputCloud(keypoint);
    f.setInputNormals(normals);
    f.setSearchMethod(tree);
    // f.setKSearch(10);
    f.setRadiusSearch(feature_r);
    f.compute(*fpfh);
}


int main(int argc, char *argv[])
{
    CmdParams params;
    parseCommandLine (argc, argv, params);

    pcl::PointCloud<PointType>::Ptr model(new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
    LocalFeatures::Ptr model_features(new LocalFeatures());
    pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
    pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
    LocalFeatures::Ptr scene_features(new LocalFeatures());

    pcl::PointCloud<PointType>::Ptr sac_result(new pcl::PointCloud<PointType>);

    //
    //  Load clouds
    //
    if (pcl::io::loadPCDFile (params.model_filename_, *model) < 0)
    {
        std::cout << "Error loading model cloud." << std::endl;
        showHelp (argv[0]);
        return (-1);
    }
    if (pcl::io::loadPCDFile (params.scene_filename_, *scene) < 0)
    {
        std::cout << "Error loading scene cloud." << std::endl;
        showHelp (argv[0]);
        return (-1);
    }

    //去除NAN点
    std::vector<int> indices_src; //保存去除的点的索引
    pcl::removeNaNFromPointCloud(*model, *model, indices_src);
    std::cout << "remove *model nan" << endl;
    pcl::removeNaNFromPointCloud(*scene, *scene, indices_src);
    std::cout << "remove *scene nan" << endl;

    switch (params.option_) {
    case 1:
        extract_keypoint1(model, model_keypoints, 0.004, 0.01, 4); // 按照leaf_size 合成一个点， radius 大约是 leaf_size 两倍
        extract_keypoint1(scene, scene_keypoints, 0.004, 0.01, 4);
        compute_fpfh_feature1(model_keypoints, model_features);
        compute_fpfh_feature1(scene_keypoints, scene_features);
        break;
    case 2:
        extractKeyPoints2(model, model_keypoints, params.model_ss_);
        extractKeyPoints2(scene, scene_keypoints, params.model_ss_);

        computeFpfhFeature2(model_keypoints, model_features, params.normal_radius_, params.feature_radius_);
        computeFpfhFeature2(scene_keypoints, scene_features, params.normal_radius_, params.feature_radius_);
        break;
    default:
        cout << "option " << params.option_ << " is not supported" << endl;
        exit(0);
    }

    cout << "model_features: " << (*model_features) << endl;

    // SAC配准
    pcl::SampleConsensusInitialAlignment<PointType, PointType, pcl::FPFHSignature33> scia;
    scia.setInputSource(model_keypoints);
    scia.setInputTarget(scene_keypoints);
    scia.setSourceFeatures(model_features);
    scia.setTargetFeatures(scene_features);

    //scia.setMinSampleDistance(0.0001);
    scia.setNumberOfSamples(params.sac_nr_samples); //设置每次迭代计算中使用的样本数量（可省）, 可节省时间
    scia.setCorrespondenceRandomness(params.sac_k);  //设置计算协方差时选择多少近邻点，该值越大，协防差越精确， 0.1
    scia.setEuclideanFitnessEpsilon(0.001);
    scia.setTransformationEpsilon(1e-10);
    scia.setRANSACIterations(30);
    scia.align(*sac_result);

    std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;
    Eigen::Matrix4f sac_trans;
    sac_trans = scia.getFinalTransformation();
    std::cout << "sac_trans: " << sac_trans << endl;

    //
    //  Visualization
    //
    pcl::visualization::PCLVisualizer viewer ("Alignment");
    viewer.setBackgroundColor(0.8f, 0.8f, 0.8f);

    if (params.show_keypoints_) {
        pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 255, 255);
        viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");

        pcl::visualization::PointCloudColorHandlerCustom<PointType> sac_result_color_handler (sac_result, 0, 0, 255);
        viewer.addPointCloud (sac_result, sac_result_color_handler, "sac_result");
        // viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
    } else {
        pcl::PointCloud<PointType>::Ptr model_trans(new pcl::PointCloud<PointType>);
        pcl::transformPointCloud(*model, *model_trans, sac_trans);
        viewer.addPointCloud(scene, "scene");

        viewer.addPointCloud(model_trans, "model_trans");

        if (!params.out_filename_.empty()) {
            pcl::PointCloud<PointType>::Ptr outTarget(new pcl::PointCloud<PointType>);

            *outTarget = (*scene) + (*model_trans);
            pcl::io::savePCDFileASCII(params.out_filename_, *outTarget);
        }
    }


    while (!viewer.wasStopped ())
    {
        viewer.spinOnce ();
    }


    return 0;
}
